#include "common.h"
#include "Wire.h"
#include "INA226.h"

InaData inaData[USB_PORT_NUMS] = { 0 };

static INA226 INA[USB_PORT_NUMS];
const uint8_t ina_addr[USB_PORT_NUMS] = { 0x40, 0x44, 0x41, 0x45 };

const uint8_t swGpios[USB_PORT_NUMS] = { SW_GPIO_1, SW_GPIO_2, SW_GPIO_3, SW_GPIO_4 };

int init_usb_power_hwmon() {
  for (int i = 0; i < USB_PORT_NUMS; i++) {
    inaData[i].current = 0;
    inaData[i].power = 0;
    inaData[i].voltage = 0;
    inaData[i].index = i;
    inaData[i].swGpio = swGpios[i];


    if (i2c_check_address(ina_addr[i])) {
      if (!INA[i].begin(ina_addr[i])) {
        inaData[i].portStatus = USB_PORT_FAIL;
        MY_INFO("USB PORT %d INIT FAIL\n", i + 1);
      } else {
        inaData[i].portStatus = USB_PORT_ON;
        MY_INFO("USB PORT %d INIT OK\n", i + 1);
      }
    } else {
      inaData[i].portStatus = USB_PORT_FAIL;
      MY_INFO("USB PORT %d INIT FAIL\n", i + 1);
    }

    INA[i].configure(INA226_AVERAGES_16, INA226_BUS_CONV_TIME_1100US, INA226_SHUNT_CONV_TIME_1100US, INA226_MODE_SHUNT_BUS_CONT);

    INA[i].calibrate(0.01, 2);
  }
  return 0;
}

int init_hwmon_gpio() {
  for (int i = 0; i < USB_PORT_NUMS; i++) {
    pinMode(inaData[i].swGpio, OUTPUT);
    digitalWrite(inaData[i].swGpio, inaData[i].portStatus == USB_PORT_ON ? HIGH : LOW);
  }
  return 0;
}

static int get_hw_value(InaData *idata) {
  idata->voltage = INA[idata->index].readBusVoltage() * 1000;
  idata->current = INA[idata->index].readShuntCurrent() * 1000;
  idata->power = (idata->current * idata->voltage) / 1000;
  return 0;
}

int switch_usb_port(uint8_t index, bool on) {
  if (index >= USB_PORT_NUMS)
    return 1;

  digitalWrite(inaData[index].swGpio, on ? HIGH : LOW);
  return 0;
}

int refresh_usb_power_hwmon() {

  int value, ret;
  char hwPath[128] = { 0 };

  for (int i = 0; i < USB_PORT_NUMS; i++) {
    if (inaData[i].portStatus == USB_PORT_ON || inaData[i].portStatus == USB_PORT_ALERT) {
      get_hw_value(&inaData[i]);
      if (inaData[i].current > inaData[i].currCutOff) {
        // 切断电路
        if (switch_usb_port(i, false) == 0) {
          inaData[i].portStatus = USB_PORT_OC;
        }
      } else if (inaData[i].current > inaData[i].currAlert) {
        inaData[i].portStatus = USB_PORT_ALERT;
      } else {
        inaData[i].portStatus = USB_PORT_ON;
      }
    } else if (inaData[i].portStatus == USB_PORT_OC) {
    } else {
      inaData[i].current = 0;
      inaData[i].power = 0;
      inaData[i].voltage = 0;
    }
  }

  return 0;
}


void i2c_begin(uint32_t clock, int sda, int scl) {
  Wire.begin(sda, scl);
  Wire.setClock(clock);
}

void i2c_end() {
  Wire.end();
}

bool i2c_check_address(uint8_t addr) {
  Wire.beginTransmission(addr);
  return (Wire.endTransmission() == 0);
}
